#include "showApiChassis.h"



c_showApiChassis::c_showApiChassis(/* args */)
{

    
}

c_showApiChassis::~c_showApiChassis()
{
}


ros::Publisher *p_path_pub;
nav_msgs::Path path;

double position_x = 0;
double position_y = 0;
double position_th =0;
#define Dt_time 0.06
void c_showApiChassis::pubMesage_ros(float lineSpeed, float angleSpeed)
{
    double len_X = lineSpeed * Dt_time * cos(position_th); // X轴移动的距离
    double len_Y = lineSpeed * Dt_time * sin(position_th); // Y轴移动的距离
    position_x += len_X;
    position_y += len_Y;
    position_th += angleSpeed * Dt_time;
    
    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = position_x;
    this_pose_stamped.pose.position.y = position_y;

    geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(position_th*57.2957795);
    this_pose_stamped.pose.orientation.x = goal_quat.x;
    this_pose_stamped.pose.orientation.y = goal_quat.y;
    this_pose_stamped.pose.orientation.z = goal_quat.z;
    this_pose_stamped.pose.orientation.w = goal_quat.w;

    this_pose_stamped.header.stamp=ros::Time::now();
    this_pose_stamped.header.frame_id="odom";
    path.poses.push_back(this_pose_stamped);

    p_path_pub->publish(path);
    ros::spinOnce();               // check for incoming messages
}


void c_showApiChassis::Callback(const MowerProtoPakg::controlMoveOrder& msg) 
{
    // printf(" move driver line speed:%f %f\r\n", msg.linespeed(), msg.angularspeed());
    pubMesage_ros(msg.linespeed(), msg.angularspeed());
}

void c_showApiChassis::loop(void)
{
    ros::NodeHandle ph;
    ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("chassis",1, true);
    p_path_pub = &path_pub;
    path.header.stamp = ros::Time::now();
    path.header.frame_id="odom";

    // recv protobuf of mower_mv    
    eCAL::protobuf::CSubscriber<MowerProtoPakg::controlMoveOrder> subscriber("chessMessage"); // ! 
    subscriber.AddReceiveCallback(std::bind(Callback, std::placeholders::_2)); 
    while (eCAL::Ok()) {
        sleep(10);
    }
    eCAL::Finalize();// finalize eCAL API
}

void c_showApiChassis::start_thread(void)
{
    thread = std::thread(&c_showApiChassis::loop, this);
}